package NursE;

import java.util.ArrayList;

import Communication.NECommunication;
import Communication.NEMain;
import Movement.NERoom;
import Movement.NEPilot;
import ObstacleTest.RunObstacle;
import ObstacleTest.ThreadObstacle;
import WayPoints.NERecorder;
import lejos.nxt.Button;
import lejos.nxt.LCD;
import lejos.nxt.SensorPort;
import lejos.nxt.Sound;
import lejos.robotics.localization.OdometryPoseProvider;
import lejos.robotics.navigation.NavPathController;
import lejos.robotics.navigation.PathController;
import lejos.robotics.navigation.Pose;
import lejos.robotics.navigation.WayPoint;

public class NENurse {
	
	/**
	 * ----------------------------------------------------------------
	 * 								Attributes
	 * ----------------------------------------------------------------
	 */
	
	/**
	 * Le point central
	 */
	final static WayPoint centerWayPoint = new WayPoint(0,0); 
	
	
	
	protected NEWheel leftWheel;
	protected NEWheel rightWheel;
	protected NEHead head;
	protected NEPilot pilot;
	NERoom destination;
	protected PathController pathController;
	protected OdometryPoseProvider poseProvider;
	protected double width;
	protected boolean isSaving;
	protected NERecorder recorder;
	protected NERoom currentRoom;

	protected NEMain main;

	protected boolean isInCenter=true;
	
	/**
	 * ----------------------------------------------------------------
	 * 								Static constructor
	 * ----------------------------------------------------------------
	 * @param main 
	 * @throws InterruptedException 
	 */
	
	public static NENurse newNurseWithLeftAndRightWheelsAndHeadAndWidth(NEMain main, NEWheel leftWheel, NEWheel rightWheel, NEHead head, double width){
		NENurse nurse = new NENurse();
		
		nurse.setLeftWheel(leftWheel);
		nurse.setRightWheel(rightWheel);
		nurse.setHead(head);
		nurse.setWidth(width);
		nurse.initPilot();
		nurse.initPathController();
		nurse.initRecorder();
		
		nurse.main = main;
		return nurse;
		
	}

	/**
	 * ----------------------------------------------------------------
	 * 								Setters
	 * ----------------------------------------------------------------
	 */

	public void setWidth(double width)
	{
		this.width=width;
	}
	
	public void setHead(NEHead head) {
		this.head = head;
		
	}

	public void setRightWheel(NEWheel rightWheel) {
		this.rightWheel = rightWheel;
		
	}

	public void setLeftWheel(NEWheel leftWheel) {
		this.leftWheel = leftWheel;
		
	}
	
	public void initPilot(){

		//this.pilot = NEPilot.newNEPilotWithWheelDiameterWidthAndLeftAndRightMotors(this.rightWheel().diameter(), this.width(), this.leftWheel().motor(), this.rightWheel().motor());
		this.pilot = NEPilot.newNEPilotWithWheelDiameterWidthAndLeftAndRightMotorsAndTouchPort(this.rightWheel().diameter(), this.width(), this.leftWheel().motor(), this.rightWheel().motor(),SensorPort.S3);
		
		this.pilot.setTravelSpeed(4);
		this.poseProvider = new OdometryPoseProvider(this.pilot);
	}
	
	public void initPathController() {
		this.pathController = new NavPathController(this.pilot());
		
	}
	
	public void initRecorder() {
		this.recorder = new NERecorder();
		
	}
	
	private void setCurrentRoom(NERoom room) {
		this.currentRoom = room;
		
	}
	
	private char tranformeIntToCapitalLetter(int i){
		switch (i){
		case 0:
			return 'A';
		case 1:
			return 'B';
		case 2:
			return 'C';
		case 3:
			return 'D';
		case 4:
			return 'E';
		case 5:
			return 'F';
		case 6:
			return 'G';
		case 7:
			return 'H';
		case 8:
			return 'I';
		case 9:
			return 'J';
		default :
			return 'v';
		}
	}
	/**
	 * ----------------------------------------------------------------
	 * 								Getters
	 * ----------------------------------------------------------------
	 */

	public NEHead head() {
		return this.head;
	}
	
	public NEPilot pilot(){
		return this.pilot;
	}
	
	public NEWheel rightWheel() {
		return this.rightWheel;
		
	}

	public NEWheel leftWheel() {
		return this.leftWheel;
		
	}
	
	public double width() {
		return this.width;
	}
	
	public WayPoint getCurrentPosition() {
		return new WayPoint(this.poseProvider.getPose().getX(), this.poseProvider.getPose().getY());
		
	}
	
	public NERoom getCurrentRoom() {
		return this.currentRoom;
		
	}
	
	public boolean isInCenter() {
		return this.isInCenter;
		
	}
	
	public boolean isAlreadyIn(NERoom destination) {
		return this.getCurrentRoom().equals(destination);
		
	}

	/**
	 * ----------------------------------------------------------------
	 * 								Movement
	 * ----------------------------------------------------------------
	 */
	
	public void moveForward() {
		this.isInCenter = false;
		this.pilot.forward();

	}
	
	public void moveBackward() {
		this.isInCenter = false;
		this.pilot.backward();
	}
	
	
	public void stop() {
		this.pilot.stop();
	}

	//nurse va suivre la route transmise par destination
	public void goTo(int i) {
		//creation d'un thread charger de la detection des obstacles
		ThreadObstacle threadObstacle = new ThreadObstacle(this);
		
		//test dans le cas ou le 1er thread ne marche pas
		//Thread threadObstacle = new Thread(new RunObstacle(this)); 
		
		threadObstacle.start();

		if(!this.isInCenter && this.currentRoom.getName()==i)
		{
			this.main.sendToServeur('X');
		}
		else if (this.isInCenter) {
			destination = recorder.getDestination(i);
			this.pathController.followRoute(destination.getPath(), false);
			this.setCurrentRoom(destination);
			this.isInCenter = false;
			this.main.sendToServeur(this.tranformeIntToCapitalLetter(i));
		}else {
			this.goToCenter();
			this.goTo(i);
		}
		//arret du thread
		threadObstacle.stopThread();
		
	}

	public void goToCenter() {
		//creation d'un thread charger de la detection des obstacles
		ThreadObstacle threadObstacle = new ThreadObstacle(this);

		//test dans le cas ou le 1er thread ne marche pas
		//Thread threadObstacle = new Thread(new RunObstacle(this)); 
		
		threadObstacle.start();
		
		LCD.clear();
		LCD.drawInt(this.currentRoom.getName(), 0, 0);		
		this.pathController.followRoute(this.currentRoom.getPathToCenter(), false);
		this.isInCenter = true;
		
		threadObstacle.stopThread();
	}
	
	public void interruptTravel()
	{
		this.pathController.interrupt();
	}
	
	public void resumeTravel()
	{
		this.pathController.resume();
	}

	/**
	 * ----------------------------------------------------------------
	 * 								Rotation
	 * ----------------------------------------------------------------
	 */
	
	public void turnLeft() {
		if (isSaving) {
			this.saveWaypoint();
		}
		this.pilot.rotate(90);
	}

	public void turnRight() {
		if (isSaving) {
			this.saveWaypoint();	
		}
		this.pilot.rotate(-90); 
	}
	

/**
 * ----------------------------------------------------------------
 * 								Obstacle Avoidance
 * ----------------------------------------------------------------
 */
	
	/**
	 * Contourne l'obstacle par la droite
	 */
	public void bypassesByTheRight(int distance)
	{
			//nurse regarde a gauche
			this.head.lookToTheLeft();
			this.moveForward();
						
			this.turnRight();	 //tourne a droite pour contourner l'obstacle		
			
			//tant qu'il voit l'obstacle il avance puis il le surpasse
			while(this.head.detection(distance))
			{
				this.moveForward();
			}			
			this.pilot.travel(5); 
			
			//nurse tourne a gauche pour contourner
			this.turnLeft();
			this.pilot.travel(6); //il avance un peu avant la detection
			
			//tant qu'il detecte l'obstacle sur sa gauche il avance puis il le surpasse
			while(this.head.detection(distance))
			{
				this.moveForward();
			}
			this.pilot.travel(7);
			
			//nurse tourne a gauche pour arriver derriere l'obstacle
			this.turnLeft();
			this.pilot.travel(9);
			
			//retourne sur son axe
			this.turnRight();
			this.head.lookToTheFront(true);
			
	}
	
	/**
	 * Contourne l'obstacle par la gauche
	 */
	public void bypassesByTheLeft(int distance)
	{
			//nurse regarde a droite
			this.head.lookToTheRight();
			this.moveForward();
			
			this.turnLeft(); //tourne a gauche pour contourner l'obstacle	
			
			//tant qu'il voit l'obstacle il avance puis il le surpasse
			while(head.detection(distance))
			{
				this.moveForward();
			}
			this.pilot.travel(5);
			
			//nurse tourne a droite pour contourner
			this.turnRight();
			this.pilot.travel(6);	//il avance un peu avant la detection			
			
			//tant qu'il detecte l'obstacle sur sa droite il avance puis il le surpasse
			while(head.detection(distance)==true)
			{
				this.moveForward();
			}			
			this.pilot.travel(7);
			
			//nurse tourne a droite pour arriver derriere l'obstacle
			this.turnRight();
			this.pilot.travel(9);
			
			//retourne sur son axe
			this.turnLeft();
			this.head.lookToTheFront(false);
		
	}
	
		/**
		 * Nurse recule en regardant ou il peut passer
		 */
		public void moveBackwardInScanning()
		{
			double speedTravel = this.pilot.getTravelSpeed();
			
			//reduction de la vitesse lors du recul
			this.pilot.setTravelSpeed(speedTravel/4);
			moveBackward();
			
			//si pas d'obstacle a droite, nurse va contourner par la droite
			if(this.head.obstacleToTheRight(50)==false)
			{
				this.stop();
				this.pilot.setTravelSpeed(speedTravel);
				this.bypassesByTheRight(30);
			}
			//sinon si pas d'obstacle a gauche, nurse va contourner par la gauche
			else if(this.head.obstacleToTheLeft(50)==false)
			{
				this.stop();
				this.pilot.setTravelSpeed(speedTravel);
				this.bypassesByTheLeft(30);
			}
			//sinon il continue de reculer
			else
			{
				moveBackward();
			}
		}
		

	
	/**
	 * ----------------------------------------------------------------
	 * 								Waypoints management
	 * ----------------------------------------------------------------
	 */
	
	public void saveWaypoint() {
		
		double x= this.poseProvider.getPose().getX();
		double y= this.poseProvider.getPose().getY();
				
		WayPoint newWaypoint = new WayPoint(x, y);
		this.recorder.addWayPoint(newWaypoint);

	}

	public void beginSave() {
		if (!this.isInCenter) {
			this.goToCenter();	
		}
		this.main.sendToServeur('Z');
		
		this.saveWaypoint();
		this.isSaving = true;
		
	}

	public void stopSave() {
		this.saveWaypoint();
		this.currentRoom = this.recorder.getCurrentDestination();
		this.recorder.recordPath();
		this.isSaving = false;
		
		this.goToCenter();
		this.main.sendToServeur('Y');
		
	}
}
